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I. INTRODUCTION 



The problem discussed in this paper is that of esti- 
mating the position and velocity in two dimensions of a 
target by means of processing passively obtained bearing 
measurements . 

A single moving observer (tracker) monitors noisy sonar 
bearings from a radiating acoustic source (target). The 
geometric configuration is depicted in Figure 1.1. 

The problem contains nonlinearities so the conventional 
linear analysis is not possible. Also as it will be shown in 
chapter IV the dynamic process remains unobservable prior to 
tracker maneuver. That requirement of observer maneuvering 
distinguishes this problem from the more usual target motion 
analysis (TMA) problem. 

In chapter II the basic concept of the Kalman filter is 
described. Chapter III describes the non-linear case 
(Extended Kalman filter) in which category the bearings only 
tracking problem belongs. 

In chapters IV and V the problem of bearings only 
tracking with nonmaneuvering and maneuvering targets is 
discussed. Some possible solutions from the literature are 
referenced, and the case of solving the problem through a 
specific approach, i.e by using a variable value of the 
system's noise covariance matrix "Q" is tested. 

Chapter VI contains the results of the computer simula- 
tions on the subject and chapter VII contains conclusions 
and possible topics for further investigation. 
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Figure 1.1 Geometrical Configuration for the B.O.T Problem. 
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II. KALMAN FILTERING BASICS 



A. HISTORY 

In 1960, R.E. Kalman provided a new way of formulating 
the least squares filtering problem using state-space 
methods [Ref. 1]. Until that time the Wiener solution of 
the optimal filter problem was applied , which was using the 
concept of the "weighting function". In effect the weighting 
function tells how the past values of the input should be 
weighted in order to determine the present value of the 
output, that is the optimal estimate. But the Wiener solu- 
tion did not lend itself very well to the corresponding 
discrete-data problem nor was it easily extended to more 
complicated problems [Ref. 2]. 

The two main features of the Kalman formulation and 
solution of the problem are: 

Vector modeling of the random processes under 
consideration. 

Recursive processing of the noisy measurements 
(input data). The key element in any recursive procedure is 
the use of the results of the previous step to aid in 
obtaining the desired result for the current step. This is 
the main feature of the Kalman filtering and the one that 
clearly distinguishes it from the weighting function 
approach, which requires arithmetic operations on all the 
past data. 

The Kalman filtering technique has become very popular 
in target tracking applications for the previous reasons 
plus the following: 
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At a given time t, the filter generates an unbi- 
ased estimate of the state vector, which means that the 
expected value of the estimate is the value of the state 
vector at time t. 

The estimate is a minimum variance estimate 
meaning that it has the property that its error covariance 
is less than or equal to that of any other linear unbiased 
estimate . 



The filter is linear and simplifies the calcula- 
tions [Ref . 3] . 

B. THE DISCRETE KALMAN FILTER 

Assume that the random process to be estimated can be 
modeled in the form: 

x(k+l)= $(k)x(k) +r*w(k) +Au(k) (2.1) 

and the observation or measurement of the process is assumed 
to occur at discrete points in time in accordance with the 
relationship : 

z(k) = H(k)x(k) + n(k) (2.2) 

where : 

x(k) = (nxl) ; is the process state at time t(k) 

$00 = (nxn) ; is the matrix relating x(k) to 

x(k+l) in the absence of forcing function. 

w(k) ; is the random forcing input at time t(k) 
considered to be an uncorrelated sequence with zero mean and 
known variance. 
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r oo = (nxp) ; is the matrix relating the random 
forcing inputs to the state at time t(k). 1 

u(k) ; is the deterministic forcing input at time 

t(k). 



/\(k) = (nxp) ; is the matrix relating the determin- 
istic inputs to the state at time t(t). 

z(k) = (mxl) ; is the measurement vector at time 

t (k) 



H(k) = (mxn) ; is the matrix which gives the noise- 
less connection between the state vector and the measurement 
equation at time t(k). 

n(k) = (mxl) ; is the measurement noise error which 
is assumed to be a white sequence with known covariance 
structure and uncorrelated with the w(k) sequence. 

The corresponding covariance matrices are given by: 2 



E [w w.' ] = 
L k I J 



E [n n'] = 

L k t 



v u, 

* l 



' Qoo 


u. = l 


(2.3) 


. o 






R(o 

< 


K - l 


(2.4) 


. o 






0 for all 


k and i 


(2.5) 



1 p<n 

2 (') denotes matrix transposition. 
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It is assumed that we have available an initial estimate 
of the process at time t(k), which is based on the knowledge 
about the process prior to time t(k). This prior estimate 
will be denoted as x(k/k-l) where the "hat" denotes esti- 
mate, and the (k/k-1) subscript means that this is our esti- 
mate prior to processing the measurement at time t(k). 

With the assumption of the prior estimate x(k/k-l), we 
now seek to use the measurement z(k) to improve the esti- 
mate. To do that we choose a linear blending of the received 
noisy measurement and the prior estimate in accordance with 
the equation: 



x(k/k) = x (k/k- 1 ) +G (k) [z (k) -H(k)x (k/k- 1 ) ] (2.6) 

where x(k/k) is the update estimate, x(k/k-l) is given by: 

x(k/k-l) = (^)(k)x(k- 1/k- 1 ) (2.7) 

and the G(k) is the blending factor. G(k) is going to be 
determined later. At this time the problem is to find a 
particular value of G(k) that yields an update estimate that 
is optimal in sne sense. The minimum Mean - Square error 
is the require*, performance criterion for that "optimiza- 
tion". To do that we need to define the term "error covari- 
ance matrix" P(k), associated with the update (a posteriori) 
estimate, which is a matrix representing the covariance of 
the difference between the true state vector x(k) and the 
estimated x(k). 

P(k) = E [(x(k)-x(k/k-l))(x(k)-x( k/k-1))'] (2.8) 

The optimization can be done by various mathematical ways as 
treated in [Ref. 4] [Ref. 2] and [Ref. 5}. The mathematical 
derivation which is omitted here shows that if 
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(2.9) 



G(k)=P(k/k-l)H' (k)[H(k)P(k/k-l)H' (k) +R(k)] 

then this is the G(k) that minimizes the mean square estima- 
tion error, and it is called the "Kalman gain" [Ref. 2]. 

Next the covariance matrix associated with the optimal 
estimate may be computed and is given by: 3 

P(k/k) = [I - G(k)H(k)]P(k/k-l) (2.10) 

Now the updated estimate x(k/k) can be easily projected 
ahead via the transition matrix by the equation: 

x(k+l/k) = <^)(k)x(k/k) (2.11) 

ignoring the contribution of w(k) because it has zero mean 
and also it is uncorrelated with the previous W's. 

Also, the equation 

P(k+l/k) =0(k)P(k/k)<£'(k) + Q(k) (2.12) 

closes the loop and now, having the needed quantities for 
the next moment with the next measurement we can start again 
as in the previous steps. 

Equations (2.6), (2,9), (2,10), (2,11), and (2,12) thus 

comprise the Kalman filter recursive equation set. 

In Figure 2.1 the Kalman filter loop is indicated. 

1 . A Simple Example 

Assume that a stationary tracker has the ability to 
obtain range measurements in both X and Y directions of a 
target moving as in Figure 2.2. 



3 (I) is the identity matrix. 
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Figure 2.1 The Kalman Filter Loop. 

Let the target be moving with a tangential velocity of 

O 

1,660 m/min so that it covers the arc of 90 in 10 minutes. 
The tracker makes its measurements every 1 min. It is 
desired to estimate the state vector of the target defined 
as X,V x ,Y, and Vy , i.e., range and velocity in X and Y 

directions. Given are: an initial estimate x(k/k-l) and its 
error covariance matrix P(k/k-l). Let them be: 
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Figure 2.2 Tracker-Target Configuration in Example. 
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10000 



(2.13) 



x (k/k- 1 ) 



and 



o 

O 

1600 



P (k/k- 1 ) 



1000 0 O O 

0 1000 0 O 

o O 1000 o 

o O O I oo o 



(2.14) 



Then we can calculate the Kalman filter gain G(k) as in 
equation (2.9) where: 



H(k) = constant 



/ o o o 

o o i o 



and R(k) has the value: 



R(k) = constant 




(2.15) 



(2.16) 



Next, given the measurement, the updated estimate is calcu- 
lated using equation (2.6) where: 



z(k) 



H, 



(2.17) 



We can see here that the updated estimate x(k/k) depends on 
the previous x(k-l/k-l) propagated for the instant (k) i.e.. 
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x(k/k-l), and another portion equal to G (k) [z (k) -H(k)x(k) ] . 
That second portion depends on the G(k) and on how much the 
estimated and the received measurements differ. 

The updated error covariance matrix P(k/k) is then 
computed using equation (2.10). The updated error covariance 
P(k/k) is going to be less than the previous P(k/k-l) since 
the filter processed an observation and thus the uncertainty 
about the estimate is less. The term [I-G(k)H(k)] is always 
less than unity if G(k) is not zero. That means that if we 
used the last observation (i.e., G(k) not zero) then the 
term in the brackets is less than unity and P(k) becomes 
less than P(k/k-l). 

Now having x(k/k) and P(k/k) we must propagate them 
for the next instant when the next measurement will be taken 
in order to be able to compare it with the real one through 
the new measurement. So we project ahead our estimate by 
the equation (2.11) where: 

O O 
O O 

(2.18) 

/ / 
o / 



00 = 



/ 

o 

o 

o 



/ 

/ 

o 

o 



and the error covariance matrix by equation (2.12) with Q(k) 
such that : 

Q(k) = r(k)E[w(k)w'(k)]f ’(k) (2.19) 

where : 
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TOo 



( 2 . 20 ) 



/ 



roo 




and w(k) is the random forcing input at time t(k) which is 
to be formulated as a white noise with known variance. 
Finally Q(k) is given by: 



Q(k) = 




o 

o 



1 

T_ 

a. 




o 

o 



o 



o 





( 2 . 21 ) 



and it counts for the uncertainty introduced by the fact 
that we do not know if the target during the next coming 
time interval will maneuver or not. A big value of w(k) 
means that the target is very likely during the propagation 
interval to maneuver. In this case the Kalman gain will also 
be large and the filter will weight the observation more 
than the propagated state. On the opposite case if w(k) is 
zero the filter assumes that the target did not maneuver 
during that interval so it weights more the last estimate 
than the measurement. In the above case it is also assumed 
that the target acceleration in X direction is uncorrelated 
to the acceleration in Y direction for simplicity. 

Having the propagated values of x(k+l/k) and 
P(k+l/k) we can start over again from the initial step. 

The above algorithm was simulated in the computer. 
The interesting result obtained is that for the case that 
the target maneuvers the choice of w(k) is very important. 
If it is small or zero the filter does not include any extra 
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uncertainty due to possible target maneuver. So at any 
moment it gives more weight to the last propagated estima- 
tion and less to the received measurement. Thus the tracking 
accuracy is not good compared with that in which it includes 
uncertainty as can be seen in the results shown in Figures 
2.3, 2.4, 2.5, 2.6, and 2.7. 
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Figure 2.3 Filter Behavior for w=0.0. 
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Figure 2.4 Filter Behavior for w=0.5. 
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Figure 2.5 Filter Behavior for w=1.0. 
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Figure 2.6 Filter Behavior for w=3.0. 
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Figure 2.7 Filter Behavior for w=10.0. 
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III. NONLINEAR ESTIMATION 



A. INTRODUCTION 



The majority of physical phenomena are nonlinear in 

nature. So as a result , usually ,the state and/or measure- 
ment equations are nonlinear. Since the basic Kalman filter 
theory deals with linear cases , it is necessary to find a 
"method" to use it in nonlinear estimation problems. 

There are two ways of solving that problem: [Ref. 4]. 

1. By deriving an optimal filter for the nonlinear 
problem or 

2. By linearizing (approximating) the problem and 
applying the linear filter theory. 

The first method is hard to follow and will involve 

complicated mathematical computations. On the other hand the 
second method is easier and the more usual. For the 

reasons above the second method will be followed in this and 

the following chapters. 



B. ANALYSIS 

In the following analysis it is assumed that both the 
state and the measurement equations are nonlinear although 
this is not always the case. 

Assume that the random process to be estimated can be 
modeled by: 



x(k+l) = a[x(k) ,u(k) ,k] +w(k) 



(3.1) 



with the measurement equation: 
z(k) = c[x(k)] + n(k) 



(3.2) 
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It is necessary to have available a nominal trajectory 
x (k) , k=0,l,2,.... about which the linearization will be 
performed. The vector function a [x (k) , u (k) , k] is expanded in 
Taylor series about the nominal trajectory x (k) . Then the 
linearized state equations can be written: 



If A(k) is defined to be the first partial derivative of the 
nonlinear function a[x (k),u(k),k], with respect to the 
state vector x(k) , i.e., 



- A(k)x <ol (k) +w(k) 

The accuracy of this approximation depends on how close 



the nominal trajectory to the actual one was selected. 

Let us now consider the measurement equation. We have: 




[x (k) ,u (k) ,k] 




(3.4) 



Then, the i j th entry of matrix A is given by: 




(3.5) 



Also ,the vector function a[x fl1 ^ (k),u(k),k] is a known 
function of k. Thus the linearized state equations can be 
written as: 



x(k+l)=A(k)x(k)+a[x 60 (k) ,u(k) ,k] 



(3.6) 



z(k) = c[x(k)J+n(k) 
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(3.7) 



Again we can expand the nonlinear vector function c about 
the nominal trajectory x (k) with the result: 



z(k) = c[x (o) (k)] 




[x(k)-x <o) (k)] +n(k) 
x <o) (k) 



Defining 



(3.8) 



H(k) t 



k 

3 , 



x <e) CW) 



(3.9) 



we can write: 

z(k)=H(k)x(k)+c[x Co) (k)] -H(k)x (o) (k) +n(k) (3.10) 

Again as in the linearized state equation, the two terms in 
the middle of the equation (3.10) are known quantities and 
they can be handled as if they were a time varying but known 
measurement bias. For simplification if we will define 1 * 

u ' (k) = a[x (o) (k) ,u(k) ,k] - A(k) x (o) (k) (3.11) 

and 

z'(k) = z (k) - c [x f< 9 (k)] +H(k)x (k) = z(k)-^(k) (3.12) 

where : 



u ( ’ ) in this case means "prime” 
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f(k) = c[< (o >(k) - H(k)x (9-) (k)] 



(3.13) 



we can rewrite equations (3.6) and (3.7) as: 

x(k+l) = A(k)x(k) + u(k) + w(k) (3.14) 

and 

z(k) = H(k)x(k) + (k) + n(k) (3.15) 

Then starting with these linearized equations, the appro- 
priate Kalman filter equations are: 

the gain equation: 



G (k) = P(k/k-l)H’ (k)[H(k)P(k/k-l)H’ (k)+R(k)] (3.16) 

the covariance of estimation error equations: 

P (k/K- 1 ) = A(k-l)P(k-l/k-l)A' (k-l)+Q(k-l) (3.17) 

P (k/k) = [I-G(k)H(k)] P(k/k-l) (3.18) 

the filter update equation: 

x(k/k) = x (k/k- 1) +G (k) [z (k) - c (x(k/k- 1) ) ] (3.19) 



and the prediction equation: 

x(k+l/k) = a[x (k/k) , u (k) , k] (3.20) 

Notice that in equations (3.19) and (3.20) the nonlinear 
state and measurement relationships are used. An alternative 
is to use the linearize relationships in which case we 
have : 
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x(k/k) = x(k/k-l)+G(k)[z(k)-H(k)x(k/k-l)] 



(3.21) 



and 



x(k+l/k) = A(k)x(k/k) + u(k) (3.22) 

One question to be answered now is how to determine the 
’’nominal" trajectory used before. One way is to use an 
approximate trajectory that is known in advance. This 
trajectory may be available from known data, or may have 
been computed by solving the state equations: 

x l \k+l) = a[x ( °^(k),u(k),k] (3.23) 

with the initial condition x (o ^(0) = E[x(0)]. Unfortunately, 
if the uncertainty in x(0) is large the solution of equation 
(3.23) may be "too far" from x(k), the linerization error 
too big and the whole method inaccurate. 



C. THE EXTENDED KALMAN FILTER 

Another possibility is to use the estimates produced by 
the filter as the nominal trajectory about which the linear- 
ization is performed. The estimator equations are again 
given by equations (3.21) and (3.22). The matrices A(k) and 
H(k) must be used to generate G(k) so that it is available 
to process z(k) when it is available. Thus the best informa- 
tion we have when H(k) must be evaluated is x(k/k-l); when 
A(k) is to be evaluated, however, x(k/k) is available. 
Hence : 
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and 



H(k) 




[ x (k U-0] 



( 3 . 25 ) 



The H(k) and A(k) matrices must be 
and not in advance since they depend on the 



computed on-line 
last estimate. 
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IV. BEARINGS ONLY TARGET MOTION ANALYSIS ^ NONMANEUVERING 

TARGET 



A. PROBLEM DEFINITION 

The problem considered here is that of estimating the 
position and velocity of a target, in two dimensions, by 
processing passively obtained bearing measurements. 

The main application area is the Antisubmarine Warfare 
area where either a surface ship tries to locate a submarine 
through its cavitation noise or sonar transmissions, or vice 
versa . 

In the following discussion we will consider a moving 
observer (own ship) that monitors noisy sonar bearings to an 
acoustic source (target), and processes these measurements 
to obtain estimates of target position and velocity. The 
geometric configuration is shown in Figure 1.1. 

B. FORMULATIONS OF THE PROBLEM 

As it was mentioned earlier the problem contains nonli- 
nearities, and the linear Kalman filter is not applicable. 
Depending on the selection of the working coordinate system 
the nonlinear term may be either the state equation or the 
measurement equation. Even models with mixed elements from 
different coordinate systems have been used. Following are 
the most commonly used formulations of the problem: 

1 . Modified Polar Coordinates 

In the modified polar (MP) coordinates the state 
vector is comprised of the following components: 

. Bearing 
. Bearing rate 

. Range rate divided by range 
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. The reciprocal of range. 

In this case the measurement equation is linear and the 
state equation nonlinear. The nonlinearities exhibited by 
the state equations are considerably more complicated than 
those exhibited by a formulation where the measurement equa- 
tion is the nonlinear. Consequently the computational load 
for this formulation is increased. Details about the modi^ 
fied polar coordinates formulation can be found in [Ref. 6]. 

2 . Mixed Coordinates 

In this case as in the previous one the surement 
equation is the linear one and the state equ ion non 
linear. The state vector consists of: 

. Bearing 
. Range 

. Velocity component in x-direction 
. Velocity component in y-direction 
Again in this formulation there is the same complexity in 
linearizing the state equation as well as computational 
load. Analysis of the mixed- coordinate formulation can be 
found in [Ref. 7]. 

Pseudo -Linear Formulatio n 

This formulation involves replacing of the measured 
bearings with pseudo - linear measurement residuals, to 
decouple the covariance computations from the estimated 
solution. The attractive feature of this method is that it 
permits a solution to the problem via linear estimation 
techniques. This formulation is similar to the Cartesian 
formulation which will be discussed in the next subsection. 
How does it differ from the Cartesian formulation can be 
found in appendix D. More details on this formulation can 
be found in [Ref. 8]. 
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4. C arte sian Coordinates Formulation 



This is the traditional way of formulating the 
problem. The state vector consists of: 

1. Range in x-direction 

2. Velocity component in x-direction 

3. Range in y-direction 

4. Velocity component in y-direction. 

The state equation is linear and the measurement equation is 
now the nonlinear part. However the exhibited nonlinearity 
is easily circumvented without complicated or lengthy compu- 
tations as it will be shown in the next section. 

Finally the cartesian coordinate formulation will be 
adapted in the following discusion mainly because of its 
simplicity . 

C. DESCRIPTION OF THE FILTER IN CARTESIAN COORDINATES 
1 . Derivat ion of t_he S tate E quations 

If we will consider the geometric configuration of 
Figure 1 and with the restriction of target and tracker 
being in the same horizontal plane, the Cartesian formula- 
tion state vector may contain relative ranges and relative 
velocities in X and Y directions. The state vector that will 
be followed in this analysis is: 



X, (t) 




'x(t) 


X a (t ) 




V* (t) 


x 3 (t) 




y(t) 


(t) 

L j 




Vy (t ) 

_ J 



(4.1) 



with : 
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(4-2) 



x(t) x t (t)-x 0 (t) 

V * (t) V xt (t)-V xo (t) 

y(t) y t (t)-y e (t) 

v y (t) (t)-v yo (t) 

where x t ( t ) , y^ ( t ) , v xt (t),v yt (t) are the target absolute 
components of position and velocity in X and Y directions, 
and x 0 (t),y o (t),v x „ (t), and v y0 (t) are the tracker absolute 
components of position and velocity. The linear differen- 
tial equations of motion of the model are given by: 






X, (t) 




X*(t) 


x a (t ) 




a„(t) 


X 5 (t ) 




x<, (t) 


1 

X 

** 

rr 

L_ 




a y (t) 



with : 



(4.3) 



’a„(t 


a *t ( t )- a »o (t) 


a-y ( t ;j 


a y t ( t )- a yo (t)^ 



(4.4) 



where a x (t) and a y (t) are the relative accelerations in both 
directions, and a xt (t),a yi (t),a ke> (t) and a yo (t) are the 
corresponding absolute acce lerat ions of target and tracker 
in both directions correspondingly. 

The solutions of the differential equations above in 
matrix notation give: 

x(t) = A(t,tO)x(tO) + u(t, )) (4.5) 
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with : 






1 

0 

0 

0 



(t-to) 

1 

0 

0 



0 

0 

1 

0 



0 

0 

(t-to) 

1 



(4.6) 



and : 



a 






• « 






u, (t,tO) 




J( t-JOa* 0)d> 

to 


u a (t,tO) 


r 


/a k Q)d> 

to 


u 3 (t,tO) 




/( t->)a y (>)d> 

Ha 


u* ( t , tO ) 




J a y ( > 


i * 




t. 



(4.7) 



and (tO) denotes any arbitrary fixed value of time. 

Although (4.5) is valid for unconstrained vehicle 
motion, solution requirements necessitate that the bearings- 
only target motion analysis be formulated under the restric- 
tive assumption of constant target velocity. [Ref. 9]. In 
this case a x-t (t) and a yt (t) become zero and u(t,tO) reduces 
to a deterministic input vector which depends only upon the 
tracker's acceleration (maneuvers). So 

u(t , tO) = -u 0 (t , tO ) (4.8) 



where : 
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(4.9) 



J 4 (t->)a xo (»d> 

J^ a x o 

jl t->)a yo OOcO 

io 

/a yo ( ^) d ^ 

ti 

th e Measur e ment Equation 

The measurement process is described by a scalar 
time varying equation of the form: 

•P(t) = h [x ( t ) ] + n ( t ) (4.10) 

where 

h[x(t)]= arctan x s (t)/xy(t) (4.11) 

and -P(t) represents the measured target bearing corrupted 
by additive measurement noise n(t). It is assumed that n(t) 
is a white noise with zero mean and known variance d 2 , 
i . e. , 

E[n(t)]= 0 (4.12) 



u o(t/U* = 



u a, (t.tO) 
u oa (t.tO) 
«•» (t,t0) 
(t,t0) 



2. Derivation of 



and 



E[n(t )n(t+>)] 



a 

<5 Di= O 
o 



(4.13) 



D. THE DISCRETE TIME MODEL 

• The previously defined model in discrete form is 
described by: 
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(4.14) 



x(k+l) = A(k)x(k) - u(k) 



and 



f(k) = h [x ( k ) ] ♦ n(k) 



(4.15) 



where : 



tive 



x(k) is the (4x1) 
range and velocity of 



state vector consisted from rela- 
the target in X and Y directions. 



A(k) is the (4x4) state transition matrix which is 
constant and given by: 



Ao o 



1 T 0 0 
0 10 0 
0 0 1 T 
0 0 0 1 



(4.16) 



u(k) is the (4x1) vector of deterministic inputs 
due to tracker's movement and given by Equation (4.9). 

^(k) is the scalar noisy bearing measurement taken 
at t ime t (k ) . 

n(k) is the scalar additive measurement noise at 
time t (k) . 

Equation (4.14) assumes that the target moves with zero 
acceleration, (non maneuvering). Also it is assumed that the 
additive measurement noise n(k) has zero mean and a known 
variance d 2 (k). Finally an initial estimate of the state 
vector and its error covariance matrix is presumed to be 
given . 
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The extended Kalman filter technique is applied to the 
problem and yields: 

x(0/0) is the initial estimate of the state vector 
which is considered to be given. 

P(0/0) is the initial estimate error covariance 
matrix which is also considered to be given. 

x(k/k-l) = A(k)x (k- 1/k- 1) -w(k) (4.17) 



is the projection ahead of the estimated state vector. 

P (k/k- 1) = A(k)P(k- 1/k- 1)A' (k) + Q(k) (4.18) 



is the projection of the error covariance matrix and Q(k) is 
the maneuver excitation covariance matrix (assumed zero if 
the target does not maneuver) . 



H(k) 




x=x(k/k- 1) 



(4.19) 



is a (1x4) matrix given by: 



H(k) = [ x j / ( x^ + Xj) ,0,-Xj / ( Xj 2 + x j ) ,0] 



(4.20) 



x=x(k/k-l) 

G(k) = P(k/k-l)H' (k) [H(k)P(k/k-l)H' (k) + cr 2 (k)]” 1 (4.21) 



is the gain equation 



■x(k/k) = x(k/k-l)+G(k) [-? (k) -hx (k/k- 1 ) ] 



(4.22) 



is the update equation and 
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P (k/k) = [I-G(k)H(k)]P(k/k-l) 



(4.23) 



is the error covariance update equation. 

The above algorithm was formulated in computer simula- 
tion program as in Appendix B and tested for the situations 
shown in Figure 4.1 and Figure 4.2. 

In the first case (Figure 4.1), the target was moving 
from east to west on a constant course and speed of 20 
m/sec, while the tracker was maneuvering following a sinuso- 
dial track with main course from east to west also, and a 
velocity of 10 m/s ec in x-direction. The target had a rela- 
tive velocity of 10 m/sec with respect the tracker in the 
X-axis and 0 m/sec in the Y-axis. 

In the second case (Figure 4.2), the target was moving 
as the first case but the tracker was following a circular 
path of radius 2000 m with a turning rate of 2° /sec. 

The measurement error was taken as zero mean and 0.1 
covariance and the measurement interval 1 sec. The behavior 
of the filter is displayed in the following Figures and is 
considered to be satisfactory. 
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Figure 4.2 Nonmaneuvering Target. Case 
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Figure 4.3 Range Error in X-Direction Case 
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Figure 4.4 Range Error in Y-Direction Case 
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Figure 4.5 Range Error Case 
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Figure 4.6 Relative Target Velocity in X-Direction Case 
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Figure 4.7 Relative Target Velocity in Y-Direction Case 
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Figure 4.8 Target Velocity in X-Direction Case 
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Figure 4.9 Target Velocity in Y-Direction Case 
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Figure 4.10 Range Error in X-Direction Case 
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Figure 4.11 Range Error in Y-Direction Case 




53 



Figure 4.12 Range Error Case 
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Figure 4.13 Relative Target Velocity in X-Direction Case 
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Figure 4.14 Relative Target Velocity in Y-Direction Case 
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Figure 4.15 Target Velocity in X-Direction Case 
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Figure 4.16 Target Velocity in Y-Direction Case 



V. MANEUVERING TARGET 



Up to this time we made the assumption that the target 
does not maneuver. However in real world applications this 
is not the usual case and hence the assumption is unreason- 
able. Specifically, in the main application area of the 
bearings-only tracking, i.e., in the A.S.W scene, it is 
expected that the target will not keep constant velocity but 
instead it will command some kind of zig-sag during the 
normal open sea transit and strong maneuvering or evasion 
after detection of a potential threat. It is evident thus 
that there is a need to accommodate the maneuvering case. 

A. POSSIBLE APPROACHES 

There are various approaches relative to the problem in 
general. Some found in the literature are following: 

1 . Variable Dimension Filter 

In this case, the filter operates in its normal mode 
in the absence of any maneuvers. A detection scheme is used 
to determine that a maneuver is indeed occuring. Once a 
maneuver is detected, a different state model is used. The 
extent of the maneuver as detected is then used to yield an 
estimate for the extra state components. The tracking is 
then continued with the augmented state model until it will 
be reverted to the normal model by another decision. The two 
models are a constant velocity and a constant acceleration 
model for the maneuvering case. Details on the analysis of 
that method can be found in [Ref. 10]. 
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2. Expanded Number of States 



In this case the model includes the acceleration 
component in it. This method has the disadvantage that if 
the target does not have acceleration, using a third order 
model increases the estimation errors for both position and 
velocity [Ref. 10]. Also, the computational load increases 
drastically by augmenting the model by one term. 

3 . Modeling Target Acceleration as Random Process of 

Known Form 

This method is based on the fact that the target 
acceleration and thus the target maneuver, is correlated in 
time; i.e., if the target is accelerating at time t, it is 
likely to be accelerating at time t+&tau for sufficiently 
small £. A typical representative model of the correla- 
tion function r( ) associated with the target acceleration 
is given by: 

r(?) = E [a(t )a(t+£)] = d 2 e ,a>0 (5.1) 

where (d 2 ) is the variance of the target acceleration and 
(a) is the reciprocal of the maneuver time constant. The 
maneuver excitation covariance matrix Q(k) then depends on 
the correlation function r^), which also depends on the type 
of the target . 

The above formulation includes the acceleration term 
in the state vector. So the performance of the filter is 
degraded by the computational overhead. The quality of the 
estimate is also degraded when the target is moving with 
constant velocity. 

Analysis of the above method can be found at 
[Ref. 11]. 
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4. Use Variable Maneuver Excitation Error Covariance 



Matrix 

The filter is modeled as a second order and it does 
not include acceleration term in it. The idea is [Ref. 12]. 
to use a set of different values for the forcing input 
covariance Q(k) . The filter monitors the innovation error 
in the equation: 

x(k/k) = x(k/k-l) + G(k)[$(k) - h(x(k/k-l) )] (5.2) 

i.e., the term [^(k) -h(x(k/k- 1 ) ) ] in every iteration. If 
that error becomes larger than a predetermined threshold, 
that means that the received bearing measurement does not 
agree with that the filter generated and was supposed to 
receive. Correspondingly, the estimated vector does not 
agree with the actual. So the filter assumes that the target 
made a maneuver. Depending on the size of the innovation 
error, a value for the excitation covariance matrix Q(k) is 
applied to the error covariance propagation equation: 

P(k/k-l) = A(k)P (k/k- 1 )A’ (k) + Q(k) (5.3) 

The effect of the above is to increase the uncertainty of 
the filter which consequently causes an increase of the gain 
G(k). The bigger the G(k) the more the filter "believes" 
the measurements rather than the previous estimates. 

So the filter is "partially" reinitialized. By 
partially is meant that the new initial estimates of the 
state vector and specifically the range terms are very close 
to the real ones estimated just before the maneuver. Thus 
the filter has good conditions to start over and estimate 
the new state after the maneuver. 
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B. BEARINGS-ONLY TRACKING WITH MANEUVERING TARGET. 



From the previously mentioned methods of dealing with 
maneuvering targets, we are going to develop the last one 
i.e., that of using a variable maneuver excitation error 
covariance matrix Q(k) . 

This method uses a four-state model, so it is faster 
than the others using a six-state models and is the simplest 
of all. Actually only a few extra lines of program are 
added to that of a nonmaneuvering target. 

1. Determination of the Q(k) Matrix 

If we will suppose that the target made a maneuver, 
(acceleration (a)) during the state propagation time from 
(k) to (k+1), in one direction say X, then the error intro- 
duced to our propagated estimate in the range term will be 
(l/2)aT 2 and the error introduced in the velocity term will 
be aT. Combining that fact in both direction and with the 
assumption that an acceleration in X is uncorrelated to an 
acceleration in Y the resulting Q(k) is given by: 



Q(k) = r(k)E[w(k)w'(k)]f ' (k) 



(5.4) 



where : 



r no = 





(5.5) 
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2. Simulation Results for Cases 3 and 4 with w= 1 , d 2 = 0 . 1 



The above 
computer. Two 
tracker as shown 



method was modeled and simulated in 
geometric configurations of target 
in Figures (5.1) and (5.2) were tested. 



the 

and 
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Figure 5.1 Maneuvering Target. Case 
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Figure 5.2 Maneuvering Target. Case 



In the case 3 the target was following a steady course from 
east to west and a constant speed of 20 m/sec. At the 700th 
second it changed course to the right and speed components 
to 12 m/ sec in X and -10 m/sec in Y direction. After 
another 700 seconds it changed course again that time from 
west to east and resumed a speed of 20 m/sec. The tracker 
was moving as in case 1 of the nonmaneuvering target. The 
intervals displayed in the Figures (5.1) and (5.2) corre- 
spond to time intervals of 100 seconds. 

In the case 4 the target was following the same 
track as in case 3 but that time the tracker was maneuvering 
as in case 2. 

In the following simulations the measurement error 
was supposed to have zero mean and 0.1 variance. The meas- 
urement interval was again taken as 1 sec which is also 
considered as reasonable for a real application. The (W) was 
taken equal to 1.0. 

The simulation program (Appendix C) for the above 
conditions gave the results shown in the following Figures 
5.3 to 5.16. In both cases the filter detected the maneuver 
and very rapidly after approximately 300 seconds estimated 
the new target parameters. The fluctuations of the errors 
due to the target maneuver are smaller than those during the 
first initialization of the problem. This can be explained 
by the fact that after the target maneuver detection the 
filter had an "accurate" reinitialization state from the 
previous tracking. 
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Figure 5.3 Range Error in X-Direction Case 
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Figure 5.4 Range Error in Y-Direction Case 
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Figure 5.5 Range Error Case 
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Figure 5.6 Relative Target Velocity in X-Direction Case 
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Figure 5.7 Relative Target Velocity in Y-Direction Cas 
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Figure 5.8 Target Velocity in X-Direction Case 
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Figure 5.9 Target Velocity in Y-Direction Case 
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Figure 5.10 Range Error in X-Direction Case 
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Figure 5.11 Range Error in Y-Direction Case 




75 



Figure 5.12 Range Error Case 
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Figure 5.13 Relative Target Velocity in X-Direction Case 
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Figure 5.14 Relative Target Velocity in Y-Direction Case 
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Figure 5.15 Target Velocity in X-Direction Case 
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Figure 5.16 Target Velocity in Y-Direction Case 
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Filter Behavior Under Different Values of w and <5 2 



In order to investigate the behavior of the filter 
for more extreme conditions, simulations where conducted 
with various values of measurement error variance (cf 2 ) and 
various values of (w) . The following combinations where 
tested for the case 3 configuration and the range error was 
obtained in each of the combinations. 



w 




0.1 


0.5 


0.1 


2.0 


0.1 


4.0 


1.0 


0.5 


1.0 


2.0 


1.0 


4.0 


3.0 


0.5 


3.0 


2.0 


3.0 


4.0 


10.0 


0.5 


10.0 


2.0 


10.0 


4.0 


In the 


following Figures 5.17 to 5.28 the filter 



behavior is displayed. It is characteristic that the filter 
tracking accuracy and quality is related to both values of 
(w) and ( c5 2 ) . For the specific configuration it came out 
that if the (d 2 ) was more than 0.5 then the filter was very 
sensitive to the value of (w) . The best results were 
obtained with the smallest tested value of w=0.1. This 
should be expected because in the case that the measurement 
noise is too big and we additionally introduce uncertainty 
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due to the target maneuver, then the filter assumes a lot of 
uncertainty and at any time behaves erratically following 
the noisy received measurements. It seems that for each 
kind of target and environmental condition (i.e. measurement 
noise variance) there will be an optimal (w) to account for 
the target maneuvers. 
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Figure 5.17 Range Error Case 
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Figure 5.18 Range Error Case 
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Figure 5.19 Range Error Case 
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Figure 5.21 Range Error Case 
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Figure 5.22 Range Error Case 
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Figure 5.23 Range Error Case 
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Figure 5.24 Range Error 
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Figure 5.25 Range Error Case 
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Figure 5.26 Range Error Case 
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Figure 5.27 Range Error Case 3, w=10. 
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Figure 5.28 Range Error Case 3, w= 10 . 



VI. CONCLUSIONS 



The proposed way of solving the problem of tracking a 
maneuvering target using noisy bearings - only measurements 
was tested and it exhibited satisfactory behavior. The main 
characteristics of it are: 

1. The filter responds satisfactorily in the case 
that the target maneuvers . The filter appears to be very 
sensitive to the value of o 2 . The results were satisfactory 
up to the value of o 2 =0.5. After that the behavior of the 
filter depends very much on the value of w. The smaller the 
value of w the better the filter tracks. 

2. The design is simple and almost no extra compu- 
tational power is needed beyond that of a nonmaneuvering 
target filter. 

3. The estimation is accurate for a nonmaneuvering 
target as well, and it does not pay the overhead of reduced 
accuracy as the other methods do in the nonmaneuvering case. 

4. Some other target- tracker configurations were 
tested which are not referenced in the previous chapters. In 
some of them the filter exhibited disability to track the 
target. In those cases the characteristic event was that the 
target was moving in such a way that even the tracker's 
maneuvers did not cause significant changes in the measured 
bearings . So the tracker maneuvers are very important in the 
bearings -only tracking problem. They must be such that will 
cause changing bearing rates. Of course the tracker's 
maneuvers are restricted by various factors as speed capa- 
bility, tactical situation, intentions (evade or attack), 
etc . 
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Possible subjects for further investigation: 

1. Analytically how does the filter behave in a 
variety of tracker- target , w - d 2 configurations? For 
example, for a given value of (d 2 ), what is the optimal (w)? 

2. In the simulations the tracker was supposed to 
move with a continuously changing course which is not the 
real case. Also the tracker was supposed to assume huge 
amounts of acceleration during its maneuvers, i.e. it was 
supposed to change course and speed in one second which also 
is not realistic. How does this assumption differ from the 
real case? 

3. Investigate the tracker motion under realistic 
constraints with the requirement of obtaining tactical 
advantage and simultaneously providing needed bearing rate 
to accurately solve the tracking problem. 

4. Investigate the effect of assuming realistic 
constraints on target motion. 

In this Thesis we dealt with the problem of maneuvering 
target passive tracking using a simple method. The first 
results are satisfactory, however the method needs further 
detailed investigation for even better performance. 
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APPENDIX A 



SIMPLE EXAMPLE SIMULATION PROGRAM 
REALM P (4 , 4 ) > H ( 2 , 4 ) » HT (4,2) ,F(4,4) ,FT(4,4) , 

*S7 (4,4) » 

“X ( 4 , 1 ) , T , TT , Z ( 2 , 1 ) , XPR(4 ,1) , PPR(4 , 4 ) ,Q(4,4),DET, 

*G (4,2), 

"hX (2,1) , ZHX (2, I) ,S1(4,2) , S2 ( 2 , 2 ) ,S6(2,2) , GH (4,4) , 
*FX(4,1) ,R1 ,R2 ,XI (4 , 1) , R(2 , 2 ) ,W,FPUP(4,4) ,FPUPFT(4,4) , 
*A1,A2 ,A3 ,R3 

INTEGER N , M , KK , I , J , K , L , NR , S 
C 

N = 4 
W=0 . 5 
M= 1 
S = 2 
NR= 15 

DO 1 I = 1 , N 
DO 1 J= 1 , N 

1 F ( I , J ) =0 . 

F ( 1 , 1 ) = 1 • 

F ( 1 , 2 ) = 1 . 

F (2 , 2 ) = 1 . 

F ( 3 , 3 ) = 1 . 

F ( 3 , 4 ) = 1 . 

F(4,4)=l. 

DO 2 1=1, N 
DO 2 J= 1 , N 

2 FT(I,J)=F(J,I) 

DO 3 1=1, N 

DO 3 J= 1 , N 

3 S7 (I , J) = 0 . 

DO 4 1=1, N 
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4 



DO 4 J= 1 , N 
S7 (I , I ) = 1 . 
X(l,l)=10000. 
X(2 , 1) =0 . 

X( 3 , 1 ) =0 . 

X (4 , 1 ) = 1600 . 

R( 1 , 1) =1 . 
R(l,2)=0. 
R(2,l)=0. 

R( 2 , 2 ) = 1 . 

DO 5 1=1, N 
DO 5 J= 1 , N 

5 P(I,J)=0. 
P(l,l)=1000. 

P (2 , 2 ) = 1000 . 

P ( 3 , 3 ) = 1000 . 

P (4 ,4) = 1000 . 

DO 6 1=1, S 
DO 6 J= 1 , N 

6 H(I , J) =0 . 

H( 1 , 1 ) = 1 . 

H(2 , 3 )=1 . 

DO 7 1=1, S 
DO 7 J=1,N 

7 HT ( J , I ) =H ( I , J ) 
DO 71 1=1, N 

DO 71 J= 1 , N 
71 Q( I , J ) =0 . 

Q(l,l)=.25 *W 
Q( 3 , 3 ) = . 25*W 
Q ( 1 , 2 ) = . 5 “ W 
Q ( 3 , 4 ) = . 5 " W 
Q ( 2 , 1)= . 5 “W 
Q ( 4 , 3 ) = . 5 " W 
Q( 2 , 2 ) =W 
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Q(4,4)=W 
DO 999 KK= 1 ,NR 
T= FLOAT (KK ) 

L=KK- 1 
TT=FLOAT (L ) 

Z ( 1 , 1 ) = 10000 . -'COS ( . 15 7 "TT ) 

Z ( 2 , 1 ) = 10000 . *SIN ( . 15 7 "'TT ) 

CALL MM ( P , HT , S 1 , N , N , S ) 

CALL MM (H ,S1,S2,S,N,S) 

DO 8 1=1, S 
DO 8 J=1,S 

8 S2(I,J)=S2(I,J)+R(I,J) 

DET=S2 ( 1 , 1 ) * S2 ( 2 , 2 ) - S2 ( 1 , 2 ) *S2 (2,1) 
S6(l, 1)=S2(2,2)/DET 
S6(1,2)=-S2(1,2)/DET 
S6(2, 1)=-S2(2, 1)/DET 
S6(2,2)=S2(1, 1)/DET 
CALL MM( SI ,S6,G,N,S,S) 

CALL MM ( H , X , HX , S , N , M ) 

DO 9 1=1, S 
DO 9 J= 1 ,M 

9 ZHX (I,J)=Z(I,J)- HX ( I , J ) 

CALL MM ( G , ZHX , XI , N , S , M ) 

DO 10 1=1, N 

DO 10 J= 1 ,M 

10 X(I,J)=X(I,J)+XI(I,J) 

CALL MM(G,H,GH,N,S,N) 

DO 11 1=1, N 

DO 11 J= 1 , N 

11 IGH ( I , J ) = S7 ( I , J ) - GH ( I , J ) 

CALL MM ( IGH , P , PUP , N , N , N ) 

CALL MM ( F , X , XPR , N , N , M ) 

CALL MM ( F , PUP , FPUP , N , N , N ) 

CALL MM (FPUP , FT , FPUPFT . N , N , N ) 

DO 13 1=1, N 



98 



13 



DO 13 J = 1 , N 

PPR ( I , J ) =FPUPFT (I,J)+Q(I,J) 

WRITE (6 , 106)Z(1,1),Z(2,1),X(1,1) ,X(3 , 1) ,XPR( 1 , 1) , 
- V XPR( 3,1) 

X ( 1 s 1 ) =XPR ( 1,1) 

X(2 , 1)=XPR(2 , 1) 

X(3 , 1)=XPR(3 ,1) 

X(4, 1)=XPR(4, 1) 

DO 14 1=1, N 
DO 14 J= 1 , N 
14 P(I, J)=PPR(I,J) 

106 FORMAT (6(F8.1)) 

999 CONTINUE 
STOP 
END 

SUBROUTINE MM (A , B,C,N1,N2,N3) 

REAL "'4 A(N1,N2) ,B(N2,N3) ,C(N1,N3) 

DO 100 1=1, N1 
DO 100 K=1,N3 
C (I ,K ) =0 . 

DO 100 J=1,N2 

100 C ( I , K ) = C ( I , K ) + A ( I , J ) “ B ( J , K ) 

RETURN 

END 
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APPENDIX B 



B.O.T NONMANEUVERING TARGET SIMULATION PROGRAM. 

REAL*8 P ( 4 , 4 ) , H ( 1 , 4 ) , HT (4, 1) ,Q(4,4) ,AX, 
*XA (1,1) , YA (1,1) , 

• V RI (1,1) , VV(4000 , 4 ) , RR(4000 ) , S7(4,4) , 

*GRT (4,4) , VXA (1,1) ,VYA(1,1) 

*, V( 1, 4000), Z( 1,1) » Y( 1 , 1 ) , SI (4 , 1 ) , 

*S2(1, 1) , XI (4 , 1 ) , TT , XI ,X3 , El , UU , 

*HH (1,1), HI (4,1) ,X(4,1) ,G(4,1) , 

*S6 (1,1) , GY (4,1) , HX (1,1) ,TY,W, 
*GHX(4,1),F(4,4),FT(4,4),XPR(4,1) , 

*PPR(4,4) , FP (4 , 4 ) , GT (1,4) ,R( 1 , 1) , 

*GH (4,4) , IGH (4,4) ,IGHT(4,4) , PUP (4, 4) , 

*IGHP (4 , 4 ) , IGHPT (4,4) ,GR(4,1) 

INTEGER N , M , NN , NR , L , KK , I , J , K , NS 

N=4 
M= 1 
NN= 1 
W=3 .DO 
NR= 2000 
NS=4000 

SB= . IDO/57 .295779D0 
SX=50 . DO 
SY= 50 . DO 
SVX=1. DO 
SVY= 1 . DO 
DS=211133 .DO 
DS1= 33 333 3 . DO 
HHH= 0 . DO 

DO 1 J= 1 , N 
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DO 1 1=1, N 
Q(I , J)=0 .DO 

1 F (I , J ) =0 . DO 
F ( 1 , 1 ) = 1 . DO 
F ( 1 , 2 ) = 1 . DO 
F (2 , 2 ) = 1 . DO 
F(3,3)=1.D0 
F ( 3 , 4 ) = 1 . DO 
F (4 , 4 ) = 1 . DO 
DO 2 1=1, N 
DO 2 J= 1 , N 

2 FT (I , J )=F( J , I ) 

C 

C GENERATION AND STORAGE OFF I. C. NOISE 

DO 10 1=1, N 
CALL GGNML (DS,NS,RR) 

DO 10 J = 1 , NN 

10 VV ( J , I )=RR( J ) 

C 

C MAKE MATRIX S7= IDENTITY. 

DO 11 1=1, N 
DO 11 J= 1 , N 

11 S7 (I , J )=0 . DO 
DO 12 1=1, N 

12 S7 ( I , I ) = 1 . DO 

C 

C START THE FIRST RUN, INITIAL STATE VALUE. 
DO 99 JJ= 1 ,NN 
X ( 1 , 1 ) = -4000 . DO 
X( 2 , 1)=+12 .DO 
X(3,1)=5000.D0 
X (4 , 1) =2 . DO 

C 

C DEFINE R AND RI MATRICES. 

R ( 1 , 1 ) = SB**2 
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RI ( 1 , 1 ) = 1 . DO / R ( 1 , 1 ) 

C INITIALIZE P MATRIX 
DO 13 1=1, N 
DO 13 J= 1 , N 

13 P ( I , J ) =0 . DO 

■ P(1,1)=SX**2 

P(2,2)=SVX**2 
P ( 3 , 3 ) = SY**2 
P(4 , 4)=SVY**2 

C GENERATION OFF MEASUREMENT NOISE - STORAGE. 

DO 14 1=1, M 

CALL GGNML(DS1 ,NS ,RR) 

DO 14 J = 1 , NR 

14 V ( I , J ) =RR( J ) 

C 

C TIME EVOLUTION 
C 

DO 999 KK= 1 , NR 
T=DFLOAT (KK ) 

L=KK- 1 
TT=DFLOAT (L ) 

C GENERATION OFF MEASUREMENT DATA 
Xl= - 5000 . D0+ 10 . DO “TT 

X3 = 8000. DO + 2000. DO-DCOS (0 . 035 D0*TT ) 

UU=X1/X3 

Z ( 1 , 1 ) = DATAN 2 ( X 1 , X 3 ) 

C ADD NOISE 

Y(1,1) = Z(1,1) + SB*V ( 1 , KK ) 

C PROJECTION OF X: XPR = X(K+1/K) = F * X(K/K)+ D*U 
CALL MM (F , X , XPR , N , N , M) 

C 

AX=0 . DO 

VY= - 70 . D0*DSIN (0 . 035D0*(TT + 0 . DO ) ) 

AY = - 2 . 45 DO -'DCOS ( 0 . 035D0*TT ) 
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c 



D ( 1 , 1 ) = 0 . DO 
D(2,1)=0.D0 
D ( 3 , 1 ) = AY / 2 . 0 DO 
D(4 , 1) =AY 

DO 77 1=1, N 
DO 77 J= 1 ,M 

77 XPR ( I , J ) =XPR (I,J)+D(I,J) 

C 

C PROJECTION OF P : PPR = P(K+1/K) = F * P(K/K) * FT + Q 
CALL MM (F , P , FP , N , N , N ) 

CALL MM(FP , FT , PPR ,N ,N , N ) 

DO 78 1=1, N 
DO 78 J= 1 , N 

78 PPR (I , J )=PPR(I ,J)+Q(I,J) 

X ( 1 , 1 ) =XPR (1,1) 

X(2 , 1)=XPR(2 , 1) 

X ( 3 , 1 ) =XPR ( 3 ,1) 

X (4 , 1 ) =XPR(4 , 1 ) 

C 

DO 68 1=1, N 
DO 68 J= 1 , N 
68 P ( I , J ) =PPR ( I , J ) 

C H- MATRIX 

U=X(1,1)**2+X(3,1)**2 
H(1,1)=X(3,1)/U 
H( 1 , 2 ) =0 . DO 
H(l,3)=-X(l,l)/U 
H( 1 ,4)=0 . DO 
C H- TRANSPOSE MATRIX 
HT (1,1) =H( 1,1) 

HT (2,1) =H( 1,2) 

HT (3,1) =H( 1,3) 

HT (4,1) =H( 1,4) 

C MEASUREMENT UPDATING 
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C COMPUTATION OF GAIN MATRIX G=P*HT/ (H*P*HT+R) 
CALL MM ( P , HT , S 1 , N , N , M ) 

CALL MM(H , Sl,S2,M,N,M) 

DO 23 1=1, M 
DO 23 J=1,M 

23 S2(I,J)=S2(I,J)+R(I,J) 

S6 ( 1 , 1 ) = 1 . DO / S2 ( 1 , 1) 

CALL MM( S 1,S6,G,N,M,M) 

C 

GT (1, 1 ) =G ( 1 , 1 ) 

GT ( 1 , 2 ) =G (2 , 1 ) 

GT(1,3)=G(3,1) 

GT (1,4) =G (4,1) 

C ERROR COVARIANCE MATRIX UPDATE: 

C P (K+ 1/K+ 1 ) = {I - G*H } *P (K + 1 / K ) 

CALL MM(G,H,GH,N,M,N) 

DO 73 I = 1 , N 
DO 73 J=1,N 

73 IGH (I,J)=S7(I,J)- GH ( I , J ) 

CALL MM ( IGH , P , IGHP , N , N , N ) 

DO 75 1=1, N 
DO 75 J= 1 , N 

75 PUP ( I , J ) = IGHP ( I , J ) 

DO 76 1=1, N 

DO 76 J= 1 ,N 

76 P ( I , J ) =PUP ( I , J ) 

C STATE UPDATE AT MEASUREMENT 

C X ( + ) = X ( - ) + G* ( Y - H ( X ( - ) ) ) ! ! BUT FOR E.K.F. 

CALL MM ( H , X , HX , M , N , M ) 

HH (1,1)=Y(1,1)- DATAN 2 ( X ( 1 , 1 ) ,X(3 , 1) ) 

CALL MM (G ,HH , XI , N , M , M ) 

DO 80 1=1, N 
DO 80 J=1,M 

80 X(I,J)=X(I,J)+XI(I,J) 

E1 = DSQRT( (Xl-X(l, 1))**2+ (X3-X(3 , 1) )**2 ) 
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E2 = (Xl-X(l, 1) ) 

E3= (X3-X(3 , 1) ) 

TY=X (4 , 1 ) -VY- AY 

WRITE (6,107)T,E2,E3,E1,X(2,1),TY 
107 FORMAT (6(3X(F14.4))) 

999 CONTINUE 

99 CONTINUE 
STOP 
END 

CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC 
SUBROUTINE MM( A , B,C,N1,N2,N3) 

REAL “'8 A(N1,N2) ,B(N2,N3) ,C(Nl,N3) 

DO 100 1=1, N1 
DO 100 K=1,N3 
C (I , K ) =0 . DO 
DO 100 J= 1 , N2 

100 C(I,K)=C(I,K)+A(I,J) *B ( J , K ) 

RETURN 
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APPENDIX C 



B.O.T MANEUVERING TARGET SIMULATION PROGRAM. 



C 

REAL* 8 P (4 , 4 ) ,H(1,4) ,HT(4,1) ,Q(4,4) ,AX,AY, 

*D (4,1), XA (1,1) , YA ( 1 , 1 ) , RI ( 1 , 1 ) , VV (4000,4) , 
*RR(4000 ) , S7 (4 , 4 ) ,GRT(4,4) ,VXA(1, 1) ,VYA(1,1) , 

*V (1,4000) , Z ( 1 , 1 ) , Y ( 1 , 1 ) , S 1 ( 4 , 1 ) , S 2 ( 1 , 1 ) , 

*XI (4,1), TT , X 1 , X 3 , E 1 , UU , HH (1,1) ,HI (4 , 1 ) , X(4 , 1 ) , 
*G ( 4 , 1 ) , S 6 ( 1 , 1 ) , GY (4,1) , HX (1,1) , TY , HHH , W , 

*GHX (4 , 1 ) , F (4 , 4 ) , FT (4,4) , XPR( 4 ,1),PPR(4,4), 

*FP(4 , 4 ) , GT ( 1 , 4 ) , R( 1 , 1 ) , GH(4 , 4 ) , IGH(4 , 4 ) , 

*IGHT (4,4) , PUP (4, 4) ,IGHP(4,4) ,IGHPT(4,4) ,GR(4,1) 
INTEGER N , M , NN , NR , L , KK , I , J , K , NS , HHHH , HHHHH 

C * 

N = 4 
M= 1 
NN= 1 

CCCCCCCCCCCCCCCCCC 
W= 1 . DO 

CCCCCCCCCCCCCCCCCC 
NR= 2000 
NS=4000 

CCCCCCCCCCCCCCCCCC 

SB= . IDO/ 57 . 295779D0 

CCCCCCCCCCCCCCCCCC 
SX= 50 . DO 
SY=50 . DO 
SVX=1. DO 
SVY= 1 . DO 
DS=211133 .DO 
DS1=333333 . DO 
HHH=0 .DO 
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c 

DO 1 J= 1 , N 
DO 1 1=1, N 
Q(I , J)=0 .DO 

1 F(I,J)=O.DO 
F ( 1 , 1 ) = 1 . DO 
F ( 1 , 2 ) = 1 . DO 
F (2 , 2 ) = 1 . DO 
F(3,3)=l.DO 
F ( 3 , 4 ) = 1 . DO 
F (4 , 4 ) = 1 . DO 
DO 2 1=1, N 
DO 2 J= 1 , N 

2 FT (I,J)=F(J,I) 

C 

C GENERATION AND STORAGE OFF I. C. NOISE 

DO 10 1=1, N 
CALL GGNML (DS,NS,RR) 

DO 10 J= 1 , NN 

10 VV ( J , I ) = RR ( J ) 

C MAKE MATRIX S7=IDENTITY . 

DO 11 1=1, N 
DO 11 J= 1 , N 

11 S7 (I , J)=0 . DO 
DO 12 1=1, N 

12 S7 (I , I )= 1 . DO 

C START THE FIRST RUN, INITIAL STATE VALUE. 
DO 99 J J= 1 , NN 
X ( 1 , 1 ) = - 4000 . DO 
X ( 2 , 1 ) =+ 12 . DO 
X(3 , 1)=5000 .DO 
X (4 , 1 ) =2 . DO 

C DEFINE R AND RI MATRICES. 

R( 1 , 1 ) =SB**2 

RI ( 1 , 1) = 1 . D0/R( 1 , 1 ) 
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C INITIALIZE P MATRIX 
DO 13 1=1, N 
DO 13 J= 1 , N 

13 P (I , J) =0 . DO 
P ( 1 , 1 ) = SX**2 
P(2,2)=SVX**2 
P ( 3 , 3 ) = SY'” V 2 
P(4,4)=SVY**2 

C GENERATION OFF MEASURMENT NOISE - STORAGE. 

DO 14 1=1, M 

CALL GGNML (DS1 , NS , RR) 

DO 14 J= 1 , NR 

14 V (I , J ) =RR( J ) 

C TIME EVOLUTION 

DO 999 KK= 1 , NR 
T=DFLOAT (KK ) 

L=KK- 1 
TT=DFLOAT (L ) 

C GENERATION OFF MEASURMENT DATA 
Xl=-5000.D0+10. DO "'TT 

X3=8000.D0+2000. DO*DCOS (0.035 D0*TT ) + 2000 . DO 
IF (KK.LT. 700) GO TO 33 
Xl= - 100 . DO + 3 . D0*TT 

X3=8000 .D0+2000 .D0*DCOS(0 . 035 D0*TT ) - 10 . D0*TT+ 9000 . DO 
IF (KK.LT. 1400. AND. KK.GE. 700) GO TO 33 
Xl=46100.D0-30. DO "TT- 6500 . D0+6500 . DO 

X3=8000 . DO +2000 . DO*DCOS (0 . 035 D0*TT ) + 7000 . DO- 12000 . DO 
33 CONTINUE 

Z ( 1 , 1)=DATAN2(X1,X3) 

C ADD NOISE 

Y ( 1 , 1 ) = Z ( 1 , 1 ) + S B * V ( 1 , KK ) 

C PROJECTION OF X: XPR = X(K+1/K) = F * X(K/K)+ D*U 
CALL MM ( F , X , XPR , N , N , M ) 

AX=0 .DO 

VY= - 70 . DO-DSIN ( 0 . 035D0' V (TT + 0 . DO ) ) 
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AY=-2 .45 DO"DCOS (0 . 035D0*TT) 

D ( 1 , 1 ) = 0 . DO 
D (2 , 1 ) =0 . DO 
D ( 3 , 1 ) =AY / 2 . 0 DO 
D(4 , 1 ) =AY 
C 

DO 77 1=1, N 
DO 77 J= 1 ,M 

77 XPR ( I , J ) =XPR( I ,J)+D(I,J) 

C PROJECTION OF P : PPR = P(K+1/K) = F * P(K/K) * FT + Q 
CALL MM ( F , P , FP , N , N , N ) 

CALL MM(FP , FT , PPR , N , N , N ) 

IF (KK.LT.600.) GO TO 86 

IF (KK.GT. 600. AND.HHHHH.LT. 1. ) GO TO 66 

IF (KK.GT. 600. AND. HHHHH.GT.l. ) GO TO 67 

66 DO 79 1=1, N 
DO 79 J= 1 , N 

79 Q( I , J ) =0 . DO 

GO TO 86 

67 Q( 1 , 1) = . 25 DO-'-'W 
Q( 1 , 2 ) = . 5 D0*W 
Q(2 , 1) = . 5 D0*W 
Q( 2 , 2 ) =W 

Q( 3 , 3 ) = . 25 DO "W 
Q( 3 , 4) = . 5 DO “W 
Q(4 , 3 ) = . 5 DO "W 
Q(4 , 4 ) =W 
86 CONTINUE 

DO 78 1=1, N 
DO 78 J= 1 , N 

78 PPR (I , J ) =PPR( I , J ) +Q(I , J ) 

X ( 1 , 1 ) = XP R ( 1 , 1 ) 

X ( 2 , 1 ) =XPR( 2 , 1 ) 

X ( 3 , 1)=XPR(3 ,1) 

X(4 , 1)=XPR(4 , 1) 
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c 

DO 68 1=1, N 
DO 68 J= 1 , N 
68 P(I, J)=PPR(I,J) 
c H- MATRIX 

U=X (1,1) **2+X ( 3,1) **2 
H(1,1)=X(3,1)/U 
H( 1 , 2 ) =0 . DO 
H(l,3)=-X(l, 1)/U 
H( 1 , 4 ) =0 . DO 
C H- TRANSPOSE MATRIX 
HT (1,1) =H (1,1) 

HT (2 , 1 ) =H( 1,2) 

HT (3,1) =H( 1,3) 

HT (4,1) =H( 1,4) 

C UPDATING AT MEASUREMENT 

C COMPUTATION OF GAIN MATRIX G=P*HT/ (H*P*HT+R ) 

CALL MM ( P , HT , S 1 , N , N , M ) 

CALL MM (H , S1,S2,M,N,M) 

DO 23 1=1, M 
DO 23 J= 1 , M 

23 S2(I, J)=S2(I,J)+R(I,J) 

S6 ( 1 , 1)=1. DO / S2 ( 1 , 1 ) 

CALL MM( Sl,S6,G,N,M,M) 

GT(1,1)=G(1,1) 

GT ( 1 , 2 ) =G ( 2 , 1) 

GT(1,3)=G(3,1) 

GT ( 1 , 4 ) =G (4 , 1 ) 

C ERROR COVARIANCE MATRIX UPDATE : P (K+ 1/K+ 1 ) 

(I - G "H } *P (K+ 1/K ) { ) 

CALL MM(G,H,GH,N,M,N) 

DO 73 1=1, N 
DO 73 J= 1 , N 

73 IGH (I,J)=S7 ( I , J ) - GH ( I , J ) 

CALL MM ( IGH , P , IGHP , N , N , N ) 
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DO 75 1=1, N 
DO 75 J=1,N 
PUP (I , J)=IGHP(I , J) 

DO 76 1=1, N 
DO 76 J= 1 , N 
76 P ( I , J ) =PUP ( I , J ) 

C STATE UPDATE AT MEASURMENT 

C X(+)=X(- )+G*(Y-H(X(-)) ) ! ! BUT FORE.K.F. 

CALL MM (H , X , HX , M , N , M ) 

HH( 1,1)=Y(1,1) -DATAN2 (X ( 1 , 1) ,X(3 ,1) ) 
HHH=HH( 1 , 1) “150 . DO 
HHHH= SNGL (HHH) 

HHHHH = I AB S ( HHHH ) 

CALL MM ( G , HH , XI , N , M , M ) 

DO 80 1=1, N 
DO 80 J= 1 , M 

80 X(I,J)=X(I,J)+XI(I,J) 

E1=DSQRT( (Xl-X(l, 1) )**2+ (X3-X(3 , 1) )**2) 

E2= (Xl-X( 1,1)) 

E3= (X3-X(3 , 1) ) 

TY=X (4 , 1) -VY 
TX=X(2 ,1)+10.D0 

C WRITE (6,107)T,E2,E3,E1,X(2,1) , X (4 , 1 ) 

C WRITE (6 , 107 )T ,X1 ,X3 

WRITE (6,107)T,TX, TY ,HH( 1,1) 

107 FORMAT (6(3X(F14.4))) 

999 CONTINUE 

99 CONTINUE 

STOP 
END 

CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC 
SUBROUTINE MM (A ,B,C,N1,N2,N3) 

REAL* 8 A(N1,N2) ,B(N2,N3) ,C(Nl,N3) 

DO 100 1=1, N1 
DO 100 K= 1 , N3 
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C(I ,K)=0 .DO 
DO 100 J=l. 2 

100 C ( I , K ) = C ( I , K ) + A ( I , J ) *B ( J , K ) 

RETURN 
END 
END 
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APPENDIX D 



PSEUDO-LINEAR FORMULATION 

If we will start we the Cartesian formulation equations 



£(k) = h[x(k)] + n(k) (D.l) 

h[x(k)J = arctan [x A (k) /x 3 (k)] (D.2) 

E [n(k)J = 0 (D . 3 ) 

( a* c = j 

(D. 4) 

o Uj 

then after algebraical manipulations yield: 

0 = H(k)x(k) + R(k)n(k) (D.5) 

where : 

A 

H(k) = [cos^(k) , -sin^(k), 0, Oj (D.6) 

R(k)c^/x i 2 (k) + x 3 2 (k) (D . 7 ) 



The nonlinearity has been embeded in the measurement noise 
If 

e(k)=R(k)n(k)=ef fective measurement noise at time kT 

(D . 8 ) 
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it can been shown [Ref. 13]. 
statistics : 



that £(k) has the following 



E [£(k)J = 0 (D.9) 

E [g 2 (k)] = R 2 (k)cr 2 (k) (D.10) 

Finally the pseudo- linear model is analogous to that of 
Cartesian formulation model with the following modifica- 
t ions : 

1. replacing H(k) with that given by equation ( C.G) 

2. replacing c r (k) with R (k/k- 1 ) <5(k) 

3. replacing 4? (k) -h [x (k/k- 1 )] with -H(k)x(k/k- 1) 
Detailed analysis on the subject can be found in 

[Ref. 8]. 
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